/*
 DC Motor driver class (BD6211F module(by Switch Science) and Toshiba TA7291)
 Hitoshi Yamada <ouch@emergeplus.jp>
 
 History:
 26/August/09 - V0.1 released.
 
 
 This library is free software; you can redistribute it and/or
 modify it under the terms of the GNU Lesser General Public
 License as published by the Free Software Foundation; either
 version 2.1 of the License, or (at your option) any later version.
 
 This library is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 Lesser General Public License for more details.
 
 You should have received a copy of the GNU Lesser General Public
 License along with this library; if not, write to the Free Software
 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include <wiring.h>
#include <DCMotor.h>

// VREF Mode Initializer
void DCMotor::Init(int FINpin, int RINpin, int VREFpin, int motorVIN, int boardVIN)
{
  Mode = VREF_MODE;
  MotorVIN = motorVIN;
  BoardVIN = boardVIN;
  FIN = FINpin;
  RIN = RINpin;
  VREF = VREFpin;
  // Set pin direction to output
  pinMode(FIN, OUTPUT);  
  pinMode(RIN, OUTPUT);
  pinMode(VREF, OUTPUT);
}

// PWM Mode Initializer
void DCMotor::Init(int FINpin, int RINpin)
{
  Mode = PWM_MODE;
  FIN = FINpin;
  RIN = RINpin;
  VREF = 0;
  // Set pin direction to output
  pinMode(FIN, OUTPUT);  
  pinMode(RIN, OUTPUT);
}
  
// Break
void DCMotor::Break(void)
{
  // PWM Mode
  if (Mode == PWM_MODE) {
    analogWrite(FIN, 255);
    analogWrite(RIN, 255);
  // VREF Mode
  } else if (Mode == VREF_MODE) {
    digitalWrite(FIN, HIGH);
    digitalWrite(RIN, HIGH);
    analogWrite(VREF, 0);
  }
}

// Break with interval
void DCMotor::Break(int interval)
{
  Break();
  delay(interval);
}

// Free
void DCMotor::Free(void)
{
  // PWM Mode
  if (Mode == PWM_MODE) {
    analogWrite(FIN, 0);
    analogWrite(RIN, 0);
  // VREF Mode
  } else if (Mode == VREF_MODE) {
    digitalWrite(FIN, LOW);
    digitalWrite(RIN, LOW);
  }
}

// Free with interval
void DCMotor::Free(int interval)
{
  Free();
  delay(interval);
}
  
// Forward
// int speed; 0 - 255
void DCMotor::Forward(int speed)
{
  // Shrink speed parameter if over value input
  if (speed > 255)
    speed = 255;
  // PWM Mode
  if (Mode == PWM_MODE) {
    analogWrite(FIN, speed);
    analogWrite(RIN, 0);
  // VREF Mode
  } else if (Mode == VREF_MODE) {
    digitalWrite(FIN, HIGH);
    digitalWrite(RIN, LOW);
    analogWrite(VREF, speed*MotorVIN/BoardVIN);
  }
}

// Forward with interval
void DCMotor::Forward(int speed, int interval)
{
  Forward(speed);
  delay(interval);
}

// Backward
// int speed; 0 - 255
void DCMotor::Backward(int speed)
{
  // Shrink speed parameter if over value input
  if (speed > 255)
    speed = 255;
  // PWM Mode
  if (Mode == PWM_MODE) {
    analogWrite(FIN, 0);
    analogWrite(RIN, speed);
  // VREF Mode
  } else if (Mode == VREF_MODE) {
    digitalWrite(FIN, LOW);
    digitalWrite(RIN, HIGH);
    analogWrite(VREF, speed*MotorVIN/BoardVIN);
  }
}

// Backward with interval
void DCMotor::Backward(int speed, int interval)
{
  Backward(speed);
  delay(interval);
}


